import lejos.nxt.*;
import lejos.robotics.navigation.*;

public class wallfollow{

	private float diff;
	private int side;
	private Pilot p;
	Thread t;
	Thread m;
	Thread d;
	master mast;

	private boolean pause;


	public wallfollow(Pilot p, master mas){
		this.p = p;	
		mast = mas;
		m = new Thread(new Move());
		d = new Thread(new Diff());
		side =-1;
		t.start();
		m.start();
		d.start();
	}

	class Move implements Runnable {
		public void run(){
			while (true){
				while(!pause){
					p.steer(diff*side,45,true);
				}
			}
		}
	}


	class Diff implements Runnable{
		public void run(){
			float first = mast.getDistance();
			try{
				Thread.sleep(150);
			}catch(InterruptedException e){
			}
			float second = mast.getDistance();		
			diff = (second - first)*15;
		}
	}


	public float getDifference(){
		return this.diff;
	}

	/**
	 * Bepaalt de draairichting van de robot
	 * @param 	left
	 * 			if(true)
	 * 				muur staat links
	 * 			else
	 * 				muur staat rechts
	 */
	public void setSide(boolean left){
		if(left)
			this.side = 1;
		else
			this.side = -1;
	}
	public int getSide(){
		return side;
	}

	public void pause(){
		pause = true;
		p.stop();
	}

	public void restart(){
		pause = false;
	}



}

